#include "WPILib.h"
#include "math.h"
#include "RMRMain.h"
#include "RMRDrive.h"
#include "RMRUtil.h"

#include "AnalogChannel.h"

bool IS_CAL = false;

/**
 * This is the main robot class for the RMR 2009 robot
 * It is based on the SimpleRobot class and is modeled after the RobotDemo file
 * 
 * It contains methods that define robot behavior during Autonomous and Operator Control modes
 * 
 */



RMRRobot::RMRRobot() :
	rmrDrive(GetWatchdog()),
	rmrAuto(GetWatchdog(), rmrDrive),
	rmrManipulator(&rmrDrive.joystick2)

{
	GetWatchdog().SetExpiration(WATCHDOG_EXPIRATION);
	file_writen = false;
		
	
	// initialize random number
	srand(time(NULL));
	
}

bool RMRRobot::RobotIsAutonomous()
{
	return IsAutonomous();
}


void RMRRobot::Autonomous(void)
{
	
	GetWatchdog().SetEnabled(true);
	
	printf("in autonomous\n");
	return;
	rmrAuto.initHardware();
	int mode = rmrAuto.GetAutoSelect();
	printf("Running Autonomous mode %d\n", mode);
	switch(mode)
	{
		case 0:
			//do nothing
			break;
		case 1:
			// Random Driving!!!
			while (IsAutonomous()) {
				// pick one of 1: Drive Straight, 2: Turn, 3: Pause
				// and call them with random parameters
				switch(RandInt(1,3))
				{
				case 1:	rmrAuto.DriveStraight(
						RandFloat(RMRAuto::AUTO_MIN_SPEED, RMRAuto::AUTO_MAX_SPEED),
						RandFloat(RMRAuto::AUTO_MIN_TIME, RMRAuto::AUTO_MAX_TIME));
						break;
				case 2: rmrAuto.Turn(
						RandFloat(RMRAuto::AUTO_MIN_TURN, RMRAuto::AUTO_MAX_TURN), 
						RandFloat(RMRAuto::AUTO_MIN_SPEED,	RMRAuto::AUTO_MAX_SPEED), 
						RandFloat(RMRAuto::AUTO_MIN_TIME, RMRAuto::AUTO_MAX_TIME));
						break;
				case 3: rmrAuto.Pause(
						RandFloat(RMRAuto::AUTO_MIN_TIME, RMRAuto::AUTO_MAX_TIME));
						break;
				}
			}
			break;
		case 2:
			// test the rmrAuto driving functions
			// Drive straight, turn right, drive straight, turn left, drive straight
			rmrAuto.DriveStraight(1.0, 2.0);	// speed 1 for 2 seconds
			rmrAuto.Turn(1.0, 1.0, 2.0);		// right turn at speed 1 for 2 seconds
			rmrAuto.DriveStraight(1.0, 2.0);	// speed 1 for 2 seconds
			rmrAuto.Turn(-1.0, 1.0, 2.0);		// left turn at speed 1 for 2 seconds
			rmrAuto.DriveStraight(1.0, 2.0);	// speed 1 for 2 seconds
			break;
		case 3:
			//do nothing (for now)
			break;
	}
	
	
	
	
	
	
	// the following is a brain dump by Paul, and might not have any relation to reality
	/*
	 * rmrAutoCommand  command[] = {new rmrDriveStraightCommand(speed, time),
	 * 							new rmrTurnLeftCommand(radius, speed, time),
	 * 							new rmrWaitCommand(time),
	 * 						};
	 * rmrAuto.LoadSequence(command);
	 * 
	 * while (IsAutonomous()) {
	 * 	rmrAuto.doAuto(); // set motor outputs to desired speed and turn for current command,
	 * 					// if done with current command, go on to next (or stop if no more)
	 * }
	 * 
	 * 
	 * ---
	 *  
	 * The rmrAutoCommand class is a base class which has public variables desired_speed, desired_turn, and desired_time
	 * the rmrDriveStraight class is derived from rmrAutoCommand, and has a constructor with args speed and time which sets the desired_* stuff
	 * similarly for rmrTurnLeftCOmmand
	 * 
	 * ---
	 * The doAuto will read the first command from the list, gather the desired speed/turn -> send to motors
	 * use time parameter in a Timer object or a Notifier object (switch to next command after the specefied time
	 * 
	 * --
	 * Remember to feed the watchdog!
	 * 
	 */
	
	/*
	 *Quick test of RMRAuto class (don't call doAuto -- until it works)
	 * 
	 * Just call these three (in sequence or after a time delay)
	 * DriveStraight(1.0, 2);
	 * TurnLeft(10.0, 1.0, 2);
	 * TurnRight(10.0, 1.0, 2);
	 * 
	 */
	

}


void RMRRobot::OperatorControl(void)
{
	
	GetWatchdog().SetEnabled(true);
	
	
	
	printf("Starting operator control...\n");
	rmrDrive.ResetEncoders();
	rmrDrive.EnablePIDs();
	//rmrDrive.DisableTractionControl();
	
	//rmrDrive.KillAdvancedControls();
	
	//rmrManipulator.InitManipulator();
	
	FILE *debug_file = fopen("log.txt", "w");
	
	while (IsOperatorControl())
	{
		

		
		
		// diagnostic stuff, choose one of these three lines
		if (DIAGNOSTIC == 1)
			rmrDrive.ShowEncoderDiagnostics();
		else if (DIAGNOSTIC == 2)
			rmrDrive.TestMotors();
		else if (DIAGNOSTIC == 3)
			rmrDrive.TestJoyButtons(rmrDrive.steering_wheel);
		else if (DIAGNOSTIC == 4)
			rmrDrive.AccelLog(debug_file);
		
		
		// This is where we drive the robot
		if (DRIVE_MODE == 1)
		{
			printf("What the hell is tank drive?!!\n");
		}
		else if (DRIVE_MODE == 2)
		{
			rmrDrive.IO();	
			rmrDrive.Tank_Drive2(); // PID control!!!
		}
		else if (DRIVE_MODE == 3)			
		{
			rmrDrive.IO();
			//printf ("caibrated:%d\n", IS_CAL);
			if(IS_CAL)
			{
				rmrDrive.Car_Drive();
			}
		}
				
		// Manipulator
		if (MANIPULATOR_ENABLED == 1)
			rmrManipulator.doManipulator();
		
		

		GetWatchdog().Feed();
	}
	

}

START_ROBOT_CLASS(RMRRobot);

